#include "comm_extend/comm_base_extend.h"

/**************************************************************************************
功能描述: 角度正则化[-pi, pi]
修改记录:
**************************************************************************************/
float AngleNormalize(const float &angle)
{
    float normalizedAngle;
    if (angle > PI){
        normalizedAngle = angle - 2 * PI;
    }
    else if (angle < -PI){
        normalizedAngle = angle + 2 * PI;
    }
    else{
        normalizedAngle = angle;
    }

    return normalizedAngle;
}

/**************************************************************************************
功能描述: 旋转矩阵转欧拉角
修改记录:
**************************************************************************************/
void Rot2Euler(const Matrix3t &R, TMPLT &roll, TMPLT &pitch, TMPLT &yaw)
{
    TMPLT sy = sqrt(R(0,0) * R(0,0) +  R(1,0) * R(1,0));
    BOOL  singular = sy < 1e-6; 
    if (!singular){
        roll  = atan2( R(2,1), R(2,2));
        pitch = atan2(-R(2,0), sy);
        yaw   = atan2( R(1,0), R(0,0));
    }
    else{
        roll  = atan2(-R(1,2), R(1,1));
        pitch = atan2(-R(2,0), sy);
        yaw   = 0;
    }   
}

Matrix3d Hat(const Vector3d &v)
{
    Matrix3d m = Matrix3d::Zero();
    m(0,1) = -v(2); m(1,0) = -m(0,1);
    m(0,2) =  v(1); m(2,0) = -m(0,2);
    m(1,2) = -v(0); m(2,1) = -m(1,2);
    return m;
}

Vector3d Vee(const Matrix3d &m)
{
    Vector3d v = Vector3d::Zero();
    v(0) = m(2,1);
    v(1) = m(0,2);
    v(2) = m(1,0);
    return v;
}

